/******************************************************************************
*	platform/platform.c
*	 by Alex Chadwick
*
*	A light weight implementation of the USB protocol stack fit for a simple
*	driver.
*
*	platform/platform.c contains code for generic system duties such as memory
*	management and logging, which can be optionally disabled.
******************************************************************************/
#include <platform/platform.h>
#include <stdarg.h>
#include <types.h>

//+=============================================================================
// Func: MemoryReserve
// Desc: Notifies the system of memory usage.
// Args: u32   length          ..
//     : void* physicalAddress ..
// Retn: void* ..
// Note: Notifies the parent system of an unavoidable memory usage.
//     : This is typically used for memory mapped IO systems, in which
//         certain addresses have special meaning.
//     : It is up to the parent system to implement whatever must be done.
//     : The return value should be a virtual address that maps to
//         the requested physical address, or NULL on error.
//     : If MEM_NO_RESERVE is defined, a dummy implementation is created.
// Auth: Chadderz; BlueChip
//==============================================================================
#ifdef MEM_NO_RESERVE
void*  MemoryReserve (u32 length, void* physicalAddress)
{
	return physicalAddress;
}
#endif // MEM_NO_RESERVE

#ifdef MEM_INTERNAL_MANAGER_DEFAULT

#define HEAP_END ((void*)0xFFFFFFFF)

typedef  struct HeapAllocation  HeapAllocation ; // forward definition for *next
typedef
	struct HeapAllocation {
		u32            Length;
		void           *Address;
		HeapAllocation *Next;
	}
HeapAllocation;

u8              Heap[0x4000] ALIGN8;  // Support a maximum of 16KB of allocations
HeapAllocation  Allocations[0x100];   // Support 256 allocations
HeapAllocation  *FirstAllocation = HEAP_END,  *FirstFreeAllocation = NULL;
u32             allocated        = 0;

//+=============================================================================
// Func: MemoryAllocate
// Desc: Allocates memory of (at least) length bytes
// Args: u32 size .. Minimum number of bytes to be allocated
// Retn: void* .. Pointer to allocated memory -or- NULL if allocation fails
// Note: If MEM_INTERNAL_MANAGER is defined, then all platforms must provide
//         an implementation which calls MemoryReserve.
// Auth: Chadderz; BlueChip
//==============================================================================
void*  MemoryAllocate (u32 size)
{
	HeapAllocation  *Current,  *Next;

	if (FirstFreeAllocation == NULL) {
		LOG_DEBUG("Platform: First memory allocation, reserving 16KiB of heap, 256 entries.\n");
		MemoryReserve(sizeof(Heap), &Heap);
		MemoryReserve(sizeof(Allocations), &Allocations);

		FirstFreeAllocation = &Allocations[0];
	}

	size += (8 - (size & 7)) & 7; // Align to 8

	if (allocated + size > sizeof(Heap)) {
		LOG("Platform: Out of memory! We should've had more heap space in platform.c.\n");
		return NULL;
	}

	if (FirstFreeAllocation == HEAP_END) {
		LOG("Platform: Out of memory! We should've had more allocations in platform.c.\n");
		return NULL;
	}
	Current = FirstAllocation;

	while (Current != HEAP_END) {
		if (Current->Next != HEAP_END) {
			if ((u32)Current->Next->Address - (u32)Current->Address - Current->Length >= size) {
				FirstFreeAllocation->Address = (void*)((u8*)Current->Address + Current->Length);
				FirstFreeAllocation->Length  = size;
				Next = FirstFreeAllocation;
				if (Next->Next == NULL)
					if ((u32)(FirstFreeAllocation + 1) < (u32)((u8*)Allocations + sizeof(Allocations)))
						FirstFreeAllocation = FirstFreeAllocation + 1;
					else
						FirstFreeAllocation = HEAP_END;
				else
					FirstFreeAllocation = Next->Next;
				Next->Next     = Current->Next;
				Current->Next  = Next;
				allocated     += size;
				LOG_DEBUGF("Platform: malloc(%#x) = %#x. (%d/%d)\n", size, Next->Address, allocated, sizeof(Heap));
				return Next->Address;
			}
			else
				Current = Current->Next;
		} else {
			if ((u32)&Heap[sizeof(Heap)] - (u32)Current->Next - Current->Length >= size) {
				FirstFreeAllocation->Address = (void*)((u8*)Current->Address + Current->Length);
				FirstFreeAllocation->Length = size;
				Next = FirstFreeAllocation;
				if (Next->Next == NULL)
					if ((u32)(FirstFreeAllocation + 1) < (u32)((u8*)Allocations + sizeof(Allocations)))
						FirstFreeAllocation = FirstFreeAllocation + 1;
					else
						FirstFreeAllocation = HEAP_END;
				else
					FirstFreeAllocation = Next->Next;
				Next->Next     = Current->Next;
				Current->Next  = Next;
				allocated     += size;
				LOG_DEBUGF("Platform: malloc(%#x) = %#x. (%d/%d)\n", size, Next->Address, allocated, sizeof(Heap));
				return Next->Address;
			}
			else {
				LOG("Platform: Out of memory! We should've had more heap space in platform.c.\n");
				LOG_DEBUGF("Platform: malloc(%#x) = %#x. (%d/%d)\n", size, NULL, allocated, sizeof(Heap));
				return NULL;
			}
		}
	}

	Next = FirstFreeAllocation->Next;
	FirstAllocation          = FirstFreeAllocation;
	FirstAllocation->Next    = HEAP_END;
	FirstAllocation->Length  = size;
	FirstAllocation->Address = &Heap;
	if (Next == NULL)
		if ((u32)(FirstFreeAllocation + 1) < (u32)((u8*)Allocations + sizeof(Allocations)))
			FirstFreeAllocation = FirstFreeAllocation + 1;
		else
			FirstFreeAllocation = HEAP_END;
	else
		FirstFreeAllocation = Next;
	allocated += size;

	LOG_DEBUGF("Platform: malloc(%#x) = %#x. (%d/%d)\n",
	           size, FirstAllocation->Address, allocated, sizeof(Heap));
	return FirstAllocation->Address;
}

//+=============================================================================
// Func: MemoryDeallocate
// Desc: Deallocates memory (allocated by MemoryAllocate) by address.
// Args: void* address .. Base address of memory to be deallocated
// Retn: -none-
// Note: If MEM_INTERNAL_MANAGER is defined, then all platforms must provide
//         an implementation.
//     : Specifying an address NOT received from MemoryAllocate will produce
//           undefined results.
// Auth: Chadderz; BlueChip
//==============================================================================
void  MemoryDeallocate (void* address)
{
	HeapAllocation  *Current,  **CurrentAddress;

	CurrentAddress = &FirstAllocation;
	Current        = FirstAllocation;

	while (Current != HEAP_END) {
		if (Current->Address == address) {
			allocated       -= Current->Length;
			*CurrentAddress  = Current->Next;
			Current->Next    = FirstFreeAllocation;
			FirstFreeAllocation = Current;
			LOG_DEBUGF("Platform: free(%#x) (%d/%d)\n", address, allocated, sizeof(Heap));
			return;
		}
		else {
			Current        = Current->Next;
			CurrentAddress = &((*CurrentAddress)->Next);
		}
	}

	LOG_DEBUGF("Platform: free(%#x) (%d/%d)\n", address, allocated, sizeof(Heap));
	LOG("Platform: Deallocated memory that was never allocated. Ignored, but you should look into it.\n");

	return;
}

//+=============================================================================
// Func: MemoryCopy
// Desc: Copies chunks of memory.
// Args: void* destination .. Destination start address
//     : void* source      .. Source start address
//     : u32   length      .. Number of bytes to copy
// Retn: -none-
// Note: Copies length bytes from source to destination.
//     : If either source or destination are NULL, the copy will be aborted
// Auth: Chadderz; BlueChip
//==============================================================================
void  MemoryCopy (void* destination, void* source, u32 length)
{
	u8  *d,  *s;

	if (length == 0) return;

	d = (u8*)destination;
	s = (u8*)source;

	if ((u32)s < (u32)d) {
		while (length-- > 0)  *d++ = *s++ ;

	} else {
		d += length;
		s += length;
		while (length-- > 0)  *--d = *--s ;
	}

	return;
}

#endif // MEM_INTERNAL_MANAGER_DEFAULT

#define FLOAT_TEXT "Floats unsupported."

//+=============================================================================
// Func: LogPrintF
// Desc: Notifies the user of progress.
// Args: char* format       .. Formatting string
//     : u32   formatLength ..
//     : VA_ARGS            .. Format parameters
// Retn: -none-
// Note: Prints a formatted string.
//     : Uses all the formtting options in printf().
//     : Uses LogPrint.
//     : Messages are truncated at 160 characters.
// Auth: Chadderz; BlueChip
//==============================================================================
#ifndef NO_LOG
void  LogPrintF (char* format, u32 formatLength, ...)
{
	va_list  args;
	char     messageBuffer[160];
	u32      messageIndex = 0;
	u32      width        = 0;
	u32      precision    = 1;
	u32      characters;
	s32      svalue;
	u32      value;
	bool     opened       = false;
	bool     flagged;                 //! sanity dictates this should be initialised to false
	bool     widthed      = false;
	bool     precisioned  = false;
	bool     length       = false;
	bool     left         = false;
	bool     plus         = false;
	bool     space        = false;
	bool     hash         = false;
	bool     zero         = false;
	bool     nwidth       = false;
	bool     period       = false;
	bool     nprecision   = false;
	bool     longInt      = false;
	bool     shortInt     = false;
	char     character;
	char     *string;
	u8       base;

	va_start(args, formatLength);

	for (u32 index = 0;
	     (index < formatLength) && (messageIndex < sizeof(messageBuffer) - 1);
	     index++) {
		if (opened) {
			if (!flagged) {
				switch (format[index]) {
					case '-':
						if (!left)  left    = true ;
						else        flagged = true ;
						break;
					case '+':
						if (!plus)  plus    = true ;
						else        flagged = true ;
						break;
					case ' ':
						if (!space)  space   = true ;
						else         flagged = true ;
						break;
					case '#':
						if (!hash)  hash    = true ;
						else        flagged = true ;
						break;
					case '0':
						if (!zero)  zero    = true ;
						else        flagged = true ;
						break;
					default:
						flagged = true;
				}
			}

			if (flagged && !widthed) {
				switch (format[index]) {
					case '0':  case '1':  case '2':  case '3':  case '4':
					case '5':  case '6':  case '7':  case '8':  case '9':
						nwidth  = true;
						width  *= 10;
						width  += format[index] - '0';
						continue;

					case '*':
						if (!nwidth) {
							widthed = true;
							width   = va_arg(args, u32);
							continue;
						} else {
							widthed = true;
						}
						break;

					default:
						widthed = true;
				}
			}

			if (flagged && widthed && !precisioned) {
				if (!period) {
					if (format[index] == '.') {
						period    = true;
						precision = 0;
						continue;

					} else {
						precisioned = true;
					}

				} else {
					switch (format[index]) {
					case '0':  case '1':  case '2':  case '3':  case '4':
					case '5':  case '6':  case '7':  case '8':  case '9':
						nprecision  = true;
						precision  *= 10;
						precision  += format[index] - '0';
						continue;

					case '*':
						if (!nprecision) {
							precisioned = true;
							precision   = va_arg(args, u32);
							continue;
						} else {
							precisioned = true;
						}
						break;

					default:
						precisioned = true;
					}
				}
			}

			if (flagged && widthed && precisioned && !length) {
				switch (format[index]) {
					case 'h':
						length   = true;
						shortInt = true;
						continue;

					case 'l':
						length  = true;
						longInt = true;
						continue;

					case 'L':
						length = true;
						continue;

					default:
						length = true;
				}
			}

			if (flagged && widthed && precisioned && length) {
				character = '%';
				base      = 16;
				switch (format[index]) {
					case 'c':
						character = va_arg(args, int) & 0x7f;
						// fallthrough: '%'
					case '%':
						messageBuffer[messageIndex++] = character;
						break;

					case 'd':
					case 'i':
						if      (shortInt)  svalue = (s32)((s16)va_arg(args, s32) & 0xFFFF) ;
						else if (longInt)   svalue = va_arg(args, s64);
						else                svalue = va_arg(args, s32);

						characters = 1;
						if (svalue < 0) {
							svalue = -svalue;
							messageBuffer[messageIndex++] = '-';

						} else if (plus) {
							messageBuffer[messageIndex++] = '-';

						} else if (space) {
							messageBuffer[messageIndex++] = ' ';

						} else {
							characters = 0;
						}

						for (u32 digits = 0;
						     (digits < precision) || (svalue > 0);
						     digits++, characters++) {
							for (u32 j = 0;  j < digits;  j++)
								if (messageIndex - j < sizeof(messageBuffer) - 1)
									messageBuffer[messageIndex - j] = messageBuffer[messageIndex - j - 1];
							if (messageIndex - digits < sizeof(messageBuffer) - 1)
								messageBuffer[messageIndex++ -digits] = '0' + (svalue % 10);
							svalue /= 10;
						}

						if (characters < width) {
							if (!left)
								for (u32 i = 0;  i <= characters;  i++) {
									if (messageIndex - characters + width - i < sizeof(messageBuffer) - 1)
										messageBuffer[messageIndex - characters + width - i] =
											messageBuffer[messageIndex - i];
								}

							for (u32 digits = characters;  characters < width;  characters++) {
								if (messageIndex - (!left ? digits : 0) < sizeof(messageBuffer) - 1)
									messageBuffer[messageIndex - (!left ? digits : 0)] = zero ? '0' : ' ';
							}
						}
						break;

					case 'e':  case 'E':
					case 'f':
					case 'g':  case 'G':
						for (u32 i = 0;
						     ((i < width) || (i < sizeof(FLOAT_TEXT))) &&
							 (messageIndex < sizeof(messageBuffer) - 1);
						     i++) {
							if (i < sizeof(FLOAT_TEXT))
								messageBuffer[messageIndex++] = FLOAT_TEXT[i];
							else
								messageBuffer[messageIndex++] = zero ? '0' : ' ';
						}
						break;

					case 'o':
						base = 8;
						// fallthrough: 'u'
					case 'u':
						if (format[index] == 'u')  base = 10 ;
						// fallthrough: 'x', 'X', 'p'
					case 'x':  case 'X':
					case 'p':
						if      (shortInt)  value = va_arg(args, u32) & 0xffff ;
						else if (longInt)   value = va_arg(args, u64) ;
						else                value = va_arg(args, u32) ;

						characters = 1;
						if (plus)        messageBuffer[messageIndex++] = '-' ;
						else if (space)  messageBuffer[messageIndex++] = ' ' ;
						else             characters = 0 ;

						if (hash) {
							if (format[index] == 'o') {
								if (messageIndex < sizeof(messageBuffer) - 1)
									messageBuffer[messageIndex++] = '0';
								characters++;
							} else if (format[index] != 'u') {
								if (messageIndex < sizeof(messageBuffer) - 1)
									messageBuffer[messageIndex++] = '0';
								characters++;
								if (messageIndex < sizeof(messageBuffer) - 1)
									messageBuffer[messageIndex++] = format[index];
								characters++;
							}
						}


						for (u32 digits = 0;
						     (digits < precision) || (value > 0);
						     digits++, characters++) {
							for (u32 j = 0;  j < digits;  j++)
								if (messageIndex - j < sizeof(messageBuffer) - 1)
									messageBuffer[messageIndex - j] = messageBuffer[messageIndex - j - 1];
							if (messageIndex - digits < sizeof(messageBuffer) - 1)
								messageBuffer[messageIndex++ -digits] =
									(value % base) >= 10 ? format[index] - ('X' - 'A') + ((value % base) - 10) : '0' + (value % base);
							value /= base;
						}

						if (characters < width) {
							if (!left)
								for (u32 i = 0;  i <= characters;  i++) {
									if (messageIndex - characters + width - i < sizeof(messageBuffer) - 1)
										messageBuffer[messageIndex - characters + width - i] =
											messageBuffer[messageIndex - i];
								}

							for (u32 digits = characters;  characters < width;  characters++) {
								if (messageIndex - (!left ? digits : 0) < sizeof(messageBuffer) - 1)
									messageBuffer[messageIndex++ - (!left ? digits : 0)] = zero ? '0' : ' ';
							}
						}
						break;

					case 's':
						string = va_arg(args, char*);
						for (u32 i = 0; messageIndex < sizeof(messageBuffer) - 1 && string[i] != '\0' && (!period || i < precision); i++) {
							messageBuffer[messageIndex++] = string[i];
						}
						break;

					case 'n':
						*va_arg(args, u32*) = messageIndex;
						break;
				}
				opened = false;
			}

		} else if (format[index] == '%') {
			opened      = true;
			flagged     = false;
			widthed     = false;
			precisioned = false;
			length      = false;
			width       = 0;
			precision   = 1;
			left        = false;
			plus        = false;
			space       = false;
			hash        = false;
			zero        = false;
			nwidth      = false;
			period      = false;
			nprecision  = false;
			longInt     = false;
			shortInt    = false;

		} else {
			messageBuffer[messageIndex++] = format[index];
		}
	}

	va_end(args);

	LogPrint(messageBuffer, messageIndex);

	return;
}
#endif // NO_LOG

//+=============================================================================
// Func: PlatformLoad
// Desc:
// Args: -none-
// Retn: -none-
// Note:
// Auth: Chadderz; BlueChip
//==============================================================================
void  PlatformLoad (void)
{
#	ifdef MEM_INTERNAL_MANAGER_DEFAULT
		FirstAllocation = HEAP_END;
		FirstFreeAllocation = NULL;
#	endif

	return;
}
